#include "MPU6050.h"
#include "iic_soft.h"
#include "math.h"
#define MPU_IIC_BUS IIC0
/*******************************************
*   MPU6050 驱动函数
*                           要想吃透一个传感器，先去查技术手册
                           我刚开始用的网上的例程，后来才发现低通滤波器有延时，
                            很要命的，一定要自己研究技术手册
* 
*
*******************************************/



/****************************变量定义区*************************************/
 

//offset
//陀螺仪矫正参数,每个陀螺仪都有零偏   ！！！！ 
//static 的作用哈哈哈
struct gyro_offset_type{
    float x;
    float y;
    float z;
};
struct gyro_offset_type gyro_offset={0,0,0};
 //****************角度参数*********************************
float Gyro_y=0,Gyro_x=0,Gyro_z=0;        //Y轴陀螺仪数据暂存
float Accel_x=0,Accel_y=0,Accel_z=0;	    //X轴加速度值暂存

float Angle_ax=0,Angle_ay=0,Angle_az=0;  //由加速度计算的加速度(弧度制)
float Angle_gy=0,Angle_gx=0,Angle_gz=0;  //由角速度计算的角速率(角度制)



/*******************************************函数定义区********************************************************************/


/**************************实现函数********************************************
*函数原型:		void MPU6050_Init()
*功　　能:	    初始化ＭＰＵ６０５０

*                   网上资料多用这种方法 包括 原来的  野火
*******************************************************************************/
void MPU6050_Init()
{

    IIC_Init(MPU_IIC_BUS);
    delay_ms(100);
    IICwriteByte(MPU_IIC_BUS,devAddr,MPU6050_RA_PWR_MGMT_1,0x01);//解除休眠
    IICwriteByte(MPU_IIC_BUS,devAddr,MPU6050_RA_SMPLRT_DIV,0x07);//采样率1Khz
    IICwriteByte(MPU_IIC_BUS,devAddr,MPU6050_RA_CONFIG,0x03);//低通滤波器设置 低通滤波器延时时间应与系统周期相似为宜（STC四轴说的），如果还不好就再减小，反正自己写的滤波很6
                        /* 低通滤波器 延迟 时间 
                        */
    IICwriteByte(MPU_IIC_BUS,devAddr,MPU6050_RA_GYRO_CONFIG,0x08);//陀螺仪测量范围+-200deg/s 不自检
                        /*GYRO 量程单位系数
                            +-250  deg/s  131  LSB/deg/s 初始化hex  0x00
                            +-500  deg/s  65.5 LSB/deg/s           0x08  *  
                            +-1000 deg/s  32.8 LSB/deg/s           0x10 
                            +-2000 deg/s  16.4 LSB/deg/s           0x18
                        */
    IICwriteByte(MPU_IIC_BUS,devAddr,MPU6050_RA_ACCEL_CONFIG,0x00);//加速度传感器工作在 2g  不自检
                            /*  Accle any axe
                            +-2  g    16384 LSB/g           0x00 *
                            +-4  g    8192  LSB/g           0x08 
                            +-8  g    4096  LSB/g           0x10
                            +-16 g    2048  LSB/g           0x18
                        */   
      
}
/**************************实现函数********************************************
*函数原型:		int GetDataMPU6050(uchar REG_Address)
*功　　能:	        简单读取ＭＰＵ６０５０
*******************************************************************************/
int16_t GetDataMPU6050(unsigned char REG_Address)
{
  char H,L;
	H=I2C_ReadOneByte(MPU_IIC_BUS,devAddr,REG_Address);
	L=I2C_ReadOneByte(MPU_IIC_BUS,devAddr,REG_Address+1);
	return (H<<8)+L;   //合成数据
}
/**************************实现函数********************************************
*函数原型:		int GetDataMPU6050(uchar REG_Address)
*功　　能:	        读取ＭＰＵ６０５０  的六个原始数据  并滤波  并 发送上位机曲线
*******************************************************************************/
void Read_MPU6050(accel_typedef* accel,gyro_typedef* gyro,posture_typedef* posture){
  
  //float angle_temp;
  accel->x= GetDataMPU6050(MPU6050_RA_ACCEL_XOUT_H);	//读取6050数据
	accel->y= GetDataMPU6050(MPU6050_RA_ACCEL_YOUT_H);		   
	accel->z= GetDataMPU6050(MPU6050_RA_ACCEL_ZOUT_H);	     
	gyro->x = GetDataMPU6050(MPU6050_RA_GYRO_XOUT_H)-gyro_offset.x;
	gyro->y = GetDataMPU6050(MPU6050_RA_GYRO_YOUT_H)-gyro_offset.y;
	gyro->z = GetDataMPU6050(MPU6050_RA_GYRO_ZOUT_H)-gyro_offset.z;	
	
        
	//更新平衡角速度
	//静止的物体，噪声很小，所以直接反三角函数计算，没有结合陀螺仪
	posture->x=atan2(accel->x,accel->z)*180/PI;                 //计算与地面的夹角
	posture->y=atan2(accel->y,accel->z)*180/PI;
        //Gyro_y=Gyro_y/16.4;
        //Kalman_Filter(angle_temp,-Gyro_y/65.5);//卡尔曼滤波
        // accel->x/=16384;
        // accel->y/=16384;
        // accel->z/=16384;

//  Angle_ax=(Accel_x)/8192;  //加速度处理	
//	Angle_ay=(Accel_y)/8192;  //转换关系8192LSB/g
//        Angle_az=(Accel_z)/8192;  //加速度量程 +-4g/S
//	Angle_gx=(Gyro_x)/65.5;   //陀螺仪处理
//	Angle_gy=(Gyro_y)/65.5;   //陀螺仪量程 +-500度/S
	////Angle_gz=(Gyro_z)/65.5;   //转换关系65.5LSB/度
//********************三角函数直接解算以供比较四元数解算精准度********************
//	AngleAx=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*57.2957795f; //后面的数字是180/PI 目的是弧度转角度
//  AngleAy=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*57.2957795f;
}
//------------------End of File----------------------------
